# Untitled - By: 凄雨冷夜孤人 - 周三 12月 2 2020

import sensor, image, lcd, time
import KPU as kpu
import gc, sys

import time,machine
from machine import Timer
from board import board_info
from fpioa_manager import *
from fpioa_manager import fm
from Maix import GPIO
from machine import UART
import utime
#↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓UART
fm.register(0, fm.fpioa.UART1_TX, force=True)
fm.register(1, fm.fpioa.UART1_RX, force=True)
uart_A = UART(UART.UART1, 1200, 8, 0, 0, timeout=1000, read_buf_len=4096)
fm.register(2,fm.fpioa.GPIO4)
fm.register(3,fm.fpioa.GPIO5)
tiggerPin=GPIO(GPIO.GPIO5, GPIO.OUT)
tiggerPin.value(1)
slaveReset=GPIO(GPIO.GPIO4, GPIO.OUT)
slaveReset.value(1)
time.sleep(1)
slaveReset.value(0)
time.sleep_ms(1)
slaveReset.value(1)
time.sleep(1)
def UARTsendMessgae(write_str):
    global tiggerPin
    uart_A.write(write_str)#串口A发送
    #print(write_str)
    tiggerPin.value(0)
    utime.sleep_ms(1)
    tiggerPin.value(1)
#↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑UART
harmfulGarbage=("有害垃圾","battery",)#这些 数组 的 [0] 位置用于读出这些物品的类别 。[0]后的位置用于和识别结果比较以确定类别
kitchenGarbage=("厨余垃圾","fruit","vegatable")
reuseableGarbage=("可回收垃圾","bottle", "can")
otherGarbage=("其它垃圾","cigaret","brick")
freq1=100

def angle_to_duty(angle,freq):#舵机角度换算为pwm占空比
    ms=(angle*(2.47-0.45)/180)+0.45#2.47和0.45和0.45是实验观察调整得到的值，刚好使得输入0~90~180时，舵机臂在最正确的角度
    duty=100*(ms/(1000/freq))
    return duty

t_1 = time.ticks_ms()
t_2 = time.ticks_ms()
ts=t_1-t_2
def detect(): #检测画面中的物品 并显示画面和信息
    global t_1
    global t_2
    global ts
    t_2=t_1######################
    t_1 = time.ticks_ms()
    ts=t_1-t_2
    message[6]=str(ts)#这四行测得目前每张图的间隔134ms左右
    img = sensor.snapshot()
    #t = time.ticks_ms()
    fmap = kpu.forward(task, img)
    #t = time.ticks_ms() - t
    plist=list(fmap[:])#plist是所有结果的置信概率的列
    plist[1]=0 #抛弃“无物品”这个识别结果，
    pmax=max(plist)#再找到最大的置信概率
    max_index=plist.index(pmax)

    if(pmax>0.5):
        pass
    else:#除去“无物品”结果后，最大的置信概率都是小于0.5的，才认为是无物品
        max_index=1
    imgDrawMessage(img)
    #img.draw_string(0,0, "%.2f : %s" %(pmax, labels[max_index].strip()), scale=2, color=(255, 0, 0))
    lcd.display(img)
    #print(labels[max_index])
    return labels[max_index]

def detectEnsureing(originResult):#多次识别，结果和第一次识别都一样就确信识别结果正确，否则认为识别有错
    garbageKind=originResult
    for ensureingTimes in range(5):
        nowResult=detect()
        if nowResult != originResult:
            garbageKind="no object"
            #print("not ensure!")
            break
    #print("ensure!")
    return garbageKind

message=[" "," "," "," "," "," "," "]
def imgDrawMessage(img):#把 需要与画面一起显示的信息 画在图片上
    vPossion=0
    for displayMessage in message:
        img.draw_string(0,vPossion, b"%s" %(displayMessage.strip()), scale=2, color=(255, 0, 0),mono_space=1)
        vPossion+=30

actionCount=0;
dict = {"fruit":"水果", "battery":"电池", "bottle": "瓶子","can":"易拉罐","vegatable":"菜叶等","cigaret":"烟头","brick":"砖瓦等"}
#["fruit", "no object", "battery", "bottle", "can", "vegatable", "cigaret", "brick"]
def action(garbageKind):#根据垃圾类型做出响应，舵机摆动
    global actionCount
    if garbageKind=="no object":
        return
    #lcd.draw_string(100, 100,garbageKind, lcd.RED)
    #time.sleep(1)
    #for kindEnsure in range(5)
    message[0]="处理中"
    #if garbageKind in harmfulGarbage:
    for delayTimes in range(10):
        detect()

    if (garbageKind in harmfulGarbage)and GarbageBin0.value():
        actionCount+=1
        message[1]=harmfulGarbage[0]
        detect()
        UARTsendMessgae(str(actionCount)+dict[garbageKind]+" "+"1"+"完成")
        for i in range(92,20-1,-4):
            pwm1.duty(angle_to_duty(i,freq1))
            #print(i)
            utime.sleep_ms(20)
        while(detect() in harmfulGarbage):
            servoShake(pwm1,(40,20))
        pwm1.duty(angle_to_duty(20,freq1))

    #elif garbageKind in kitchenGarbage:#带有桶满逻辑的条件是(garbageKind in kitchenGarbage)and GarbageBin0.value():
    elif (garbageKind in kitchenGarbage)and GarbageBin1.value():
        actionCount+=1
        UARTsendMessgae(str(actionCount)+dict[garbageKind]+" "+"1"+"完成")
        message[1]=kitchenGarbage[0]
        detect()
        for i in range(92,160+1,4):
            pwm1.duty(angle_to_duty(i,freq1))
            #print(i)
            utime.sleep_ms(20)
        while(detect()in kitchenGarbage):
            servoShake(pwm1,(140,160))
        pwm1.duty(angle_to_duty(160,freq1))

    #elif garbageKind in reuseableGarbage:
    elif (garbageKind in reuseableGarbage)and GarbageBin2.value():
        actionCount+=1
        UARTsendMessgae(str(actionCount)+dict[garbageKind]+" "+"1"+"完成")
        message[1]=reuseableGarbage[0]
        detect()
        for i in range(92,160+1,4):
            pwm2.duty(angle_to_duty(i,freq1))
            #print(i)
            utime.sleep_ms(20)
        while(detect()in reuseableGarbage):
            servoShake(pwm2,(160,140))
        pwm2.duty(angle_to_duty(140,freq1))
    #elif garbageKind in otherGarbage:
    elif (garbageKind in otherGarbage)and GarbageBin3.value():
        actionCount+=1
        UARTsendMessgae(str(actionCount)+dict[garbageKind]+" "+"1"+"完成")
        message[1]=otherGarbage[0]
        detect()
        for i in range(92,20-1,-4):
            pwm2.duty(angle_to_duty(i,freq1))
            #print(i)
            utime.sleep_ms(20)
        #time.sleep(1)
        while(detect()in otherGarbage):
            servoShake(pwm2,(40,20))
        pwm2.duty(angle_to_duty(20,freq1))
    for delayTimes in range(5):
        detect()
    message[0]="处理完成"
    message[1]="无物品"
    pwm1.duty(angle_to_duty(89,freq1))
    pwm2.duty(angle_to_duty(92,freq1))

#servoShakingCount=0
servoShakingState=False
def servoShake(servoName,twoAngle):
#让舵机在两个角度twoangel[0]或[1]之间来回摆，产生抖动效果。每进入一次这个函数，twoAngle[1]或[0]变为twoAngle[0]或[1]
    #global servoShakingCount
    global servoShakingState#全局变量 保留上次在这个函数里产生的值
    servoName.duty(angle_to_duty(twoAngle[servoShakingState],freq1))
    servoShakingState=not servoShakingState
    #servoShakingState取反，下次进入函数时就是相反的值，产生重复进入这个函数时servoShakingState一次为0，一次为1，来回变化的效果

def checkGarbageFull():#检测四个红外对射管的输入，判断垃圾桶是否装满
    if GarbageBin0.value()==0:
        message[2]="有害垃圾已满"
    else:
        message[2]=" "

    if GarbageBin1.value()==0:
        message[3]="厨余垃圾已满"
    else:
        message[3]=" "

    if GarbageBin2.value()==0:
        message[4]="可回收垃圾已满"
    else:
        message[4]=" "

    if GarbageBin3.value()==0:
        message[5]="其它垃圾已满"
    else:
        message[5]=" "
    pass#另外三个垃圾桶的红外对射管，等机械结构确定后再接线

image.font_load(image.UTF8, 16, 16, 0x500000)
fm.register(17, fm.fpioa.GPIO0)
fm.register(16, fm.fpioa.GPIO1)
fm.register(15, fm.fpioa.GPIO2)
fm.register(14, fm.fpioa.GPIO3)
GarbageBin0=GPIO(GPIO.GPIO0, GPIO.IN,GPIO.PULL_UP)#红外对射管的输入
GarbageBin1=GPIO(GPIO.GPIO1, GPIO.IN,GPIO.PULL_UP)#红外对射管的输入
GarbageBin2=GPIO(GPIO.GPIO2, GPIO.IN,GPIO.PULL_UP)#红外对射管的输入
GarbageBin3=GPIO(GPIO.GPIO3, GPIO.IN,GPIO.PULL_UP)#红外对射管的输入
tim1= Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim2= Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
pwm1= machine.PWM(tim1, freq=freq1, duty=angle_to_duty(89,freq1), pin=12)#舵机1的脉宽信号，接在12脚
pwm2= machine.PWM(tim2, freq=freq1, duty=angle_to_duty(92,freq1), pin=13)#舵机2的脉宽信号，接在13脚
task = kpu.load(0x300000)#,model_addr="/sd/m.kmodel",
def main(labels = None, sensor_window=(224, 224), lcd_rotation=0, sensor_hmirror=1, sensor_vflip=1):
    sensor.reset(dual_buff=False)
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QVGA)
    sensor.set_windowing(sensor_window)
    sensor.set_auto_whitebal(1)
    sensor.set_hmirror(sensor_hmirror)
    sensor.set_vflip(sensor_vflip)
    sensor.run(1)
    lcd.init(type=1)
    lcd.rotation(lcd_rotation)
    lcd.clear(lcd.WHITE)

    img = image.Image(size=(320, 240))
    img.draw_string(90, 110, "loading model...", color=(255, 255, 255), scale=2)
    lcd.display(img)

    while(True):
        #t = time.ticks_ms()
        message[0]=" "
        message[1]="无物品"
        garbageKind=detect()
        checkGarbageFull()
        if garbageKind !="no object":
            message[0]="检测中"
            message[1]=" "
            garbageKind=detectEnsureing(garbageKind)
            #message[1]=garbageKind
            action(garbageKind)
        else:
            #message[1]="no object"
            pass
        #t = time.ticks_ms()-t
        #message[6]=str(t)

    kpu.deinit(task)


if __name__ == "__main__":
    try:
        labels = ["fruit", "no object", "battery", "bottle", "can", "vegatable", "cigaret", "brick"]
        main(labels=labels)# model_addr=0x300000
        #main(labels=labels, model_addr="/sd/m.kmodel")
    finally:
        gc.collect()
